
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_iomcu.c
  * @author     baiyang
  * @date       2021-12-26
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_iomcu.h"

#include <string.h>
#include <stddef.h>

#include <common/grapilot.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>

#if HAL_WITH_IO_MCU

/*-----------------------------------macro------------------------------------*/
#define IOMCU_SERIAL_RX_BUFSZ 128

/**
 * @brief   Returns an event mask from an event identifier.
 */
#define EVENT_MASK(eid) ((eventmask_t)1 << (eventmask_t)(eid))

#define IOMCU_DEBUG_ENABLE 0

#if IOMCU_DEBUG_ENABLE
#include <stdio.h>
#define debug(fmt, args ...)  do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define debug(fmt, args ...)
#endif

// max number of consecutve protocol failures we accept before raising
// an error
#define IOMCU_MAX_REPEATED_FAILURES 20

#ifndef offsetof
# define offsetof(T, x) ((size_t) &((T *)0)->x)
#endif
#ifndef container_of
#define container_of(p, T, x) ((T *)((char *)(p) - offsetof(T,x)))
#endif
/*----------------------------------typedef-----------------------------------*/
// pending IO events to send, used as an event mask
enum ioevents {
    IOEVENT_INIT=1,
    IOEVENT_SEND_PWM_OUT,
    IOEVENT_FORCE_SAFETY_OFF,
    IOEVENT_FORCE_SAFETY_ON,
    IOEVENT_SET_ONESHOT_ON,
    IOEVENT_SET_BRUSHED_ON,
    IOEVENT_SET_RATES,
    IOEVENT_ENABLE_SBUS,
    IOEVENT_SET_HEATER_TARGET,
    IOEVENT_SET_DEFAULT_RATE,
    IOEVENT_SET_SAFETY_MASK,
    IOEVENT_MIXING,
    IOEVENT_GPIO,
};

/*---------------------------------prototype----------------------------------*/
static bool iomcu_interface_init();
static void thread_main(void* parameter);
static bool _thread_create(const char *name, void (*entry)(void *parameter), uint32_t stack_size, uint8_t  priority);
static bool check_crc(void);
static void trigger_event(uint8_t event);
static void send_servo_out();
static bool read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs);
static inline bool write_register(uint8_t page, uint8_t offset, uint16_t v);
static bool write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs);
static inline void discard_input(void);
static rt_size_t write_wait(const uint8_t *pkt, uint8_t len);
static rt_err_t uart_tx_done(rt_device_t dev, void* buffer);
static rt_err_t uart_rx_indicate(rt_device_t dev, rt_size_t size);
static bool wait_timeout(uint16_t n, uint32_t timeout_ms);
static void handle_repeated_failures(void);
static void event_failed(uint32_t event_mask);
static bool modify_register(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
static void read_rc_input();
static void read_status();
static void check_iomcu_reset(void);
static void read_servo();
static void update_safety_options(void);
static void send_rc_protocols();
/*----------------------------------variable----------------------------------*/
static const char *fw_name = "io_firmware.bin";

// channel group masks
static const uint8_t ch_masks[3] = { 0x03,0x0C,0xF0 };

static iomcu intermcu;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void iomcu_ctor()
{
    intermcu.last_safety_options = 0xFFFF;
    intermcu.rate.default_freq   = 50;
}

static bool iomcu_interface_init()
{
    rt_err_t ret = RT_EOK;
    rt_uint16_t oflag = 0;              //设备使用模式标志位

    intermcu.puart = rt_device_find(GP_IOMCU_DEVICE_NAME);  //IO MCU使用的串口

    if (intermcu.puart == NULL) {
        return false;
    }

#ifdef GP_IOMCU_TX_USING_DMA
    oflag = RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_DMA_TX;
#else
    oflag = RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_TX;
#endif
#ifdef GP_IOMCU_RX_USING_DMA
    oflag |= RT_DEVICE_FLAG_DMA_RX;
#else
    oflag |= RT_DEVICE_FLAG_INT_RX;
#endif

    struct serial_configure conf = RT_SERIAL_CONFIG_DEFAULT;

    // uart runs at 1.5MBit
    conf.baud_rate = 1500*1000;
    conf.bufsz = IOMCU_SERIAL_RX_BUFSZ;

    ret |= rt_device_control(intermcu.puart, RT_DEVICE_CTRL_CONFIG, &conf); //修改参数

    ret |= rt_device_open(intermcu.puart, oflag);

    if (oflag & RT_DEVICE_FLAG_DMA_TX) //DMA模式下需要信号量触发回调
    {
        intermcu.tx_sem = rt_sem_create("iomcu", 0, RT_IPC_FLAG_FIFO);
        rt_device_set_tx_complete(intermcu.puart, uart_tx_done);
    }

    rt_device_set_rx_indicate(intermcu.puart, uart_rx_indicate);

    return ret == RT_EOK;
}

/*
  initialise library, starting thread
 */
bool iomcu_init(void)
{
    if (!iomcu_interface_init()) {
        return false;
    }

    if (brd_io_enabled() == 1) {
        check_crc();
    } else {
        intermcu.crc_is_ok = true;
    }

    rt_event_init(&intermcu.io_event, "iomcu", RT_IPC_FLAG_FIFO);

    if (!_thread_create("IOMCU", thread_main, 1024, PRIORITY_BOOST)) {
        console_panic("Unable to allocate IOMCU thread");
    }
    intermcu.initialised = true;

    return true;
}

void iomcu_write_channel(uint8_t chan, uint16_t pwm)
{
    if (chan >= IOMCU_MAX_CHANNELS) {
        return;
    }
    if (chan >= intermcu.pwm_out.num_channels) {
        intermcu.pwm_out.num_channels = chan+1;
    }
    intermcu.pwm_out.pwm[chan] = pwm;
    if (!intermcu.corked) {
        iomcu_push();
    }
}

// get state of safety switch
enum safety_state iomcu_get_safety_switch_state(void)
{
    return intermcu.reg_status.flag_safety_off?SAFETY_ARMED:SAFETY_DISARMED;
}

// force safety on
bool iomcu_force_safety_on(void)
{
    trigger_event(IOEVENT_FORCE_SAFETY_ON);
    intermcu.safety_forced_off = false;
    return true;
}

// force safety off
void iomcu_force_safety_off(void)
{
    trigger_event(IOEVENT_FORCE_SAFETY_OFF);
    intermcu.safety_forced_off = true;
}

// read from one channel
uint16_t iomcu_read_channel(uint8_t chan)
{
    return intermcu.pwm_in.pwm[chan];
}

// cork output
void iomcu_cork(void)
{
    intermcu.corked = true;
}

// push output
void iomcu_push(void)
{
    trigger_event(IOEVENT_SEND_PWM_OUT);
    intermcu.corked = false;
}

// set output frequency
void iomcu_set_freq(uint16_t chmask, uint16_t freq)
{
    // ensure mask is legal for the timer layout
    for (uint8_t i=0; i<ARRAY_SIZE(ch_masks); i++) {
        if (chmask & ch_masks[i]) {
            chmask |= ch_masks[i];
        }
    }
    intermcu.rate.freq = freq;
    intermcu.rate.chmask |= chmask;
    trigger_event(IOEVENT_SET_RATES);
}

// get output frequency
uint16_t iomcu_get_freq(uint16_t chan)
{
    if ((1U<<chan) & intermcu.rate.chmask) {
        return intermcu.rate.freq;
    }
    return intermcu.rate.default_freq;
}

// enable SBUS out
bool iomcu_enable_sbus_out(uint16_t rate_hz)
{
    intermcu.rate.sbus_rate_hz = rate_hz;
    trigger_event(IOEVENT_ENABLE_SBUS);
    return true;
}

/*
  check for new RC input
*/
bool iomcu_check_rcinput(uint64_t *last_frame_us, uint8_t *num_channels, uint16_t *channels, uint8_t max_chan)
{
    if (*last_frame_us != (uint64_t)((uint64_t)intermcu.rc_last_input_ms * 1000U)) {
        *num_channels = MIN(MIN(intermcu.rc_input.count, IOMCU_MAX_CHANNELS), max_chan);
        rt_memcpy(channels, intermcu.rc_input.pwm, (*num_channels)*2);
        *last_frame_us = (uint64_t)((uint64_t)intermcu.rc_last_input_ms * 1000U);
        return true;
    }
    return false;
}

// set IMU heater target
void iomcu_set_heater_duty_cycle(uint8_t duty_cycle)
{
    intermcu.heater_duty_cycle = duty_cycle;
    trigger_event(IOEVENT_SET_HEATER_TARGET);
}

// set default output rate
void iomcu_set_default_rate(uint16_t rate_hz)
{
    if (intermcu.rate.default_freq != rate_hz) {
        intermcu.rate.default_freq = rate_hz;
        trigger_event(IOEVENT_SET_DEFAULT_RATE);
    }
}

// setup for oneshot mode
void iomcu_set_oneshot_mode(void)
{
    trigger_event(IOEVENT_SET_ONESHOT_ON);
    intermcu.rate.oneshot_enabled = true;
}

// setup for brushed mode
void iomcu_set_brushed_mode(void)
{
    trigger_event(IOEVENT_SET_BRUSHED_ON);
    intermcu.rate.brushed_enabled = true;
}

/*
  set the pwm to use when in FMU failsafe
 */
void iomcu_set_failsafe_pwm(uint16_t chmask, uint16_t period_us)
{
    bool changed = false;
    for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
        if (chmask & (1U<<i)) {
            if (intermcu.pwm_out.failsafe_pwm[i] != period_us) {
                intermcu.pwm_out.failsafe_pwm[i] = period_us;
                changed = true;
            }
        }
    }
    if (changed) {
        intermcu.pwm_out.failsafe_pwm_set++;
    }
}


// set mask of channels that ignore safety state
void iomcu_set_safety_mask(uint16_t chmask)
{
    if (intermcu.pwm_out.safety_mask != chmask) {
        intermcu.pwm_out.safety_mask = chmask;
        trigger_event(IOEVENT_SET_SAFETY_MASK);
    }
}

/*
  check that IO is healthy. This should be used in arming checks
 */
bool iomcu_healthy(void)
{
    return intermcu.crc_is_ok && intermcu.protocol_fail_count == 0 && !intermcu.detected_io_reset && intermcu.read_status_errors < intermcu.read_status_ok/128U;
}

/*
  shutdown protocol, ready for reboot
 */
void iomcu_shutdown(void)
{
    intermcu.do_shutdown = true;
    while (!intermcu.done_shutdown) {
        rt_thread_mdelay(1);
    }
}

/*
  request bind on a DSM radio
 */
void iomcu_bind_dsm(uint8_t mode)
{
    if (!intermcu.is_chibios_backend || brd_get_soft_armed()) {
        // only with ChibiOS IO firmware, and disarmed
        return;
    }
    uint16_t reg = mode;
    write_registers(PAGE_SETUP, PAGE_REG_SETUP_DSM_BIND, 1, &reg);
}

/*
  return the RC protocol name
 */
const char *iomcu_get_rc_protocol(void)
{
    if (!intermcu.is_chibios_backend) {
        return NULL;
    }
    return "IBUS";
}

// get receiver RSSI
int16_t iomcu_get_RSSI(void)
{
    return intermcu.rc_input.rssi;
}

// Check if pin number is valid for GPIO
bool iomcu_valid_GPIO_pin(uint8_t pin)
{
    return iomcu_convert_pin_number(&pin);
}

// convert external pin numbers 101 to 108 to internal 0 to 7
bool iomcu_convert_pin_number(uint8_t* pin)
{
    if (*pin < 101) {
        return false;
    }
    *pin -= 101;
    if (*pin > 7) {
        return false;
    }
    return true;
}

// set GPIO mask of channels setup for output
void iomcu_set_GPIO_mask(uint8_t mask)
{
    if (mask == intermcu.GPIO.channel_mask) {
        return;
    }
    intermcu.GPIO.channel_mask = mask;
    trigger_event(IOEVENT_GPIO);
}

// write to a output pin
void iomcu_write_GPIO(uint8_t pin, bool value)
{
    if (!iomcu_convert_pin_number(&pin)) {
        return;
    }
    if (value == ((intermcu.GPIO.output_mask & (1U << pin)) != 0)) {
        return;
    }
    if (value) {
        intermcu.GPIO.output_mask |= (1U << pin);
    } else {
        intermcu.GPIO.output_mask &= ~(1U << pin);
    }
    trigger_event(IOEVENT_GPIO);
}

// toggle a output pin
void iomcu_toggle_GPIO(uint8_t pin)
{
    if (!iomcu_convert_pin_number(&pin)) {
        return;
    }
    intermcu.GPIO.output_mask ^= (1U << pin);
    trigger_event(IOEVENT_GPIO);
}

uint8_t iomcu_get_ch_masks_size()
{
    return ARRAY_SIZE(ch_masks);
}

const uint8_t* iomcu_ch_masks()
{
    return ch_masks;
}

/*
  main IO thread loop
 */
static void thread_main(void* parameter)
{
    rt_err_t res;

    rt_event_send(&intermcu.io_event, intermcu.initial_event_mask);
    trigger_event(IOEVENT_INIT);

    while (!intermcu.do_shutdown) {
        eventmask_t mask;
        res = rt_event_recv(&intermcu.io_event, ~0, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 
                                rt_tick_from_millisecond(10), &mask);

        // check for pending IO events
        if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT)) {
            send_servo_out();
        }
        mask &= ~(EVENT_MASK(IOEVENT_SEND_PWM_OUT));


        if (mask & EVENT_MASK(IOEVENT_INIT)) {
            // get protocol version
            if (!read_registers(PAGE_CONFIG, 0, sizeof(intermcu.config)/2, (uint16_t *)&intermcu.config)) {
                event_failed(mask);
                continue;
            }
            intermcu.is_chibios_backend = (intermcu.config.protocol_version == IOMCU_PROTOCOL_VERSION &&
                                  intermcu.config.protocol_version2 == IOMCU_PROTOCOL_VERSION2);

            // set IO_ARM_OK and FMU_ARMED
            if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
                                 P_SETUP_ARMING_IO_ARM_OK |
                                 P_SETUP_ARMING_FMU_ARMED |
                                 P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_INIT));

        if (mask & EVENT_MASK(IOEVENT_MIXING)) {
            if (!write_registers(PAGE_MIXING, 0, sizeof(intermcu.mixing)/2, (const uint16_t *)&intermcu.mixing)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_MIXING));

        if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF));

        if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_FORCE_SAFETY_ON));

        if (mask & EVENT_MASK(IOEVENT_SET_RATES)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, intermcu.rate.freq) ||
                !write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, intermcu.rate.chmask)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_SET_RATES));

        if (mask & EVENT_MASK(IOEVENT_ENABLE_SBUS)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_SBUS_RATE, intermcu.rate.sbus_rate_hz) ||
                !modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0,
                                 P_SETUP_FEATURES_SBUS1_OUT)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_ENABLE_SBUS));

        if (mask & EVENT_MASK(IOEVENT_SET_HEATER_TARGET)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, intermcu.heater_duty_cycle)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_SET_HEATER_TARGET));

        if (mask & EVENT_MASK(IOEVENT_SET_DEFAULT_RATE)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_DEFAULTRATE, intermcu.rate.default_freq)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_SET_DEFAULT_RATE));

        if (mask & EVENT_MASK(IOEVENT_SET_ONESHOT_ON)) {
            if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_SET_ONESHOT_ON));

        if (mask & EVENT_MASK(IOEVENT_SET_BRUSHED_ON)) {
            if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_SET_BRUSHED_ON));

        if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) {
            if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, intermcu.pwm_out.safety_mask)) {
                event_failed(mask);
                continue;
            }
        }
        mask &= ~(EVENT_MASK(IOEVENT_SET_SAFETY_MASK));

        if (intermcu.is_chibios_backend) {
            if (mask & EVENT_MASK(IOEVENT_GPIO)) {
                if (!write_registers(PAGE_GPIO, 0, sizeof(intermcu.GPIO)/sizeof(uint16_t), (const uint16_t*)&intermcu.GPIO)) {
                    event_failed(mask);
                    continue;
                }
            }
            mask &= ~(EVENT_MASK(IOEVENT_GPIO));
        }

        // check for regular timed events
        uint32_t now = time_millis();
        if (now - intermcu.last_rc_read_ms > 20) {
            // read RC input at 50Hz
            read_rc_input();
            intermcu.last_rc_read_ms = time_millis();
        }

        if (now - intermcu.last_status_read_ms > 50) {
            // read status at 20Hz
            read_status();
            intermcu.last_status_read_ms = time_millis();
        }

        if (now - intermcu.last_servo_read_ms > 50) {
            // read servo out at 20Hz
            read_servo();
            intermcu.last_servo_read_ms = time_millis();
        }

        if (now - intermcu.last_safety_option_check_ms > 1000) {
            update_safety_options();
            intermcu.last_safety_option_check_ms = now;
        }

        // update failsafe pwm
        if (intermcu.pwm_out.failsafe_pwm_set != intermcu.pwm_out.failsafe_pwm_sent) {
            uint8_t set = intermcu.pwm_out.failsafe_pwm_set;
            if (write_registers(PAGE_FAILSAFE_PWM, 0, IOMCU_MAX_CHANNELS, intermcu.pwm_out.failsafe_pwm)) {
                intermcu.pwm_out.failsafe_pwm_sent = set;
            }
        }

        send_rc_protocols();

    }
    intermcu.done_shutdown = true;
}

static bool _thread_create(const char *name, void (*entry)(void *parameter), uint32_t stack_size, uint8_t  priority)
{
    rt_err_t res = RT_ERROR;

    intermcu.thread_ctx = rt_thread_create(name, entry, NULL, stack_size, priority, 1);

    if (intermcu.thread_ctx != NULL) {
        res = rt_thread_startup(intermcu.thread_ctx);
    }

    return res == RT_EOK;
}

/*
  check ROMFS firmware against CRC on IOMCU, and if incorrect then upload new firmware
 */
static bool check_crc(void)
{
#if 0
    // flash size minus 4k bootloader
    const uint32_t flash_size = 0x10000 - 0x1000;

    fw = AP_ROMFS::find_decompress(fw_name, fw_size);
    if (!fw) {
        hal.console->printf("failed to find %s\n", fw_name);
        return false;
    }
    uint32_t crc = crc32_small(0, fw, fw_size);

    // pad CRC to max size
    for (uint32_t i=0; i<flash_size-fw_size; i++) {
        uint8_t b = 0xff;
        crc = crc32_small(crc, &b, 1);
    }

    uint32_t io_crc = 0;
    uint8_t tries = 32;
    while (tries--) {
        if (read_registers(PAGE_SETUP, PAGE_REG_SETUP_CRC, 2, (uint16_t *)&io_crc)) {
            break;
        }
    }
    if (io_crc == crc) {
        hal.console->printf("IOMCU: CRC ok\n");
        crc_is_ok = true;
        AP_ROMFS::free(fw);
        fw = nullptr;
        return true;
    } else {
        hal.console->printf("IOMCU: CRC mismatch expected: 0x%X got: 0x%X\n", (unsigned)crc, (unsigned)io_crc);
    }

    const uint16_t magic = REBOOT_BL_MAGIC;
    write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);

    if (!upload_fw()) {
        AP_ROMFS::free(fw);
        fw = nullptr;
        AP_BoardConfig::config_error("Failed to update IO firmware");
    }

    AP_ROMFS::free(fw);
    fw = nullptr;
#endif
    return false;
}

// trigger an ioevent
static void trigger_event(uint8_t event)
{
    if (intermcu.thread_ctx != NULL) {
        rt_event_send(&intermcu.io_event, EVENT_MASK(event));
    } else {
        // thread isn't started yet, trigger this event once it is started
        intermcu.initial_event_mask |= EVENT_MASK(event);
    }
}

/*
  send servo output data
 */
static void send_servo_out()
{
#if 0
    // simple method to test IO failsafe
    if (AP_HAL::millis() > 30000) {
        return;
    }
#endif
    if (intermcu.pwm_out.num_channels > 0) {
        uint8_t n = intermcu.pwm_out.num_channels;
        if (intermcu.rate.sbus_rate_hz == 0) {
            n = MIN(n, 8);
        } else {
            n = MIN(n, IOMCU_MAX_CHANNELS);
        }
        uint64_t now = time_micros64();
        if (now - intermcu.last_servo_out_us >= 2000) {
            // don't send data at more than 500Hz
            if (write_registers(PAGE_DIRECT_PWM, 0, n, intermcu.pwm_out.pwm)) {
                intermcu.last_servo_out_us = now;
            }
        }
    }
}

/*
  read count 16 bit registers
*/
static bool read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs)
{
    while (count > PKT_MAX_REGS) {
        if (!read_registers(page, offset, PKT_MAX_REGS, regs)) {
            return false;
        }
        offset += PKT_MAX_REGS;
        count -= PKT_MAX_REGS;
        regs += PKT_MAX_REGS;
    }

    struct IOPacket pkt;

    discard_input();

    rt_memset(&pkt.regs[0], 0, count*2);

    pkt.code = CODE_READ;
    pkt.count = count;
    pkt.page = page;
    pkt.offset = offset;
    pkt.crc = 0;

    uint8_t pkt_size = IOPacket_get_size(&pkt);
    if (intermcu.is_chibios_backend) {
        /*
          the original read protocol is a bit strange, as it
          unnecessarily sends the same size packet that it expects to
          receive. This means reading a large number of registers
          wastes a lot of serial bandwidth. We avoid this overhead
          when we know we are talking to a ChibiOS backend
        */
        pkt_size = 4;
    }

    pkt.crc = math_crc_crc8((const uint8_t *)&pkt, pkt_size);

    rt_size_t ret = write_wait((uint8_t *)&pkt, pkt_size);

    if (ret != pkt_size) {
        debug("write failed1 %u %u %u\n", (unsigned)pkt_size, page, offset);
        intermcu.protocol_fail_count++;
        return false;
    }

    // wait for the expected number of reply bytes or timeout
    if (!wait_timeout(count*2+4, 10)) {
        debug("t=%u timeout read page=%u offset=%u count=%u\n",
              time_millis(), page, offset, count);
        intermcu.protocol_fail_count++;
        return false;
    }

    uint8_t *b = (uint8_t *)&pkt;
    uint8_t n = intermcu.rx_buff_length;
    if (n < offsetof(struct IOPacket, regs)) {
        debug("t=%u small pkt %u\n", time_millis(), n);
        intermcu.protocol_fail_count++;
        return false;
    }
    if (IOPacket_get_size(&pkt) != n) {
        debug("t=%u bad len %u %u\n", time_millis(), n, IOPacket_get_size(&pkt));
        intermcu.protocol_fail_count++;
        return false;
    }
    for (uint8_t i=0; i<n; i++) {
        if (i < sizeof(pkt)) {
            rt_device_read(intermcu.puart, 0, &b[i], 1);
        }
    }

    uint8_t got_crc = pkt.crc;
    pkt.crc = 0;
    uint8_t expected_crc = math_crc_crc8((const uint8_t *)&pkt, IOPacket_get_size(&pkt));
    if (got_crc != expected_crc) {
        debug("t=%u bad crc %02x should be %02x n=%u %u/%u/%u\n",
              time_millis(), got_crc, expected_crc,
              n, page, offset, count);
        intermcu.protocol_fail_count++;
        return false;
    }

    if (pkt.code != CODE_SUCCESS) {
        debug("bad code %02x read %u/%u/%u\n", pkt.code, page, offset, count);
        intermcu.protocol_fail_count++;
        return false;
    }
    if (pkt.count < count) {
        debug("bad count %u read %u/%u/%u n=%u\n", pkt.count, page, offset, count, n);
        intermcu.protocol_fail_count++;
        return false;
    }
    rt_memcpy(regs, pkt.regs, count*2);
    if (intermcu.protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
        handle_repeated_failures();
    }
    intermcu.total_errors += intermcu.protocol_fail_count;
    intermcu.protocol_fail_count = 0;
    intermcu.protocol_count++;
    return true;
}

/*
  write count 16 bit registers
*/
static bool write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
{
    while (count > PKT_MAX_REGS) {
        if (!write_registers(page, offset, PKT_MAX_REGS, regs)) {
            return false;
        }
        offset += PKT_MAX_REGS;
        count -= PKT_MAX_REGS;
        regs += PKT_MAX_REGS;
    }
    struct IOPacket pkt;

    discard_input();

    rt_memset(&pkt.regs[0], 0, count*2);

    pkt.code = CODE_WRITE;
    pkt.count = count;
    pkt.page = page;
    pkt.offset = offset;
    pkt.crc = 0;
    rt_memcpy(pkt.regs, regs, 2*count);
    pkt.crc = math_crc_crc8((const uint8_t *)&pkt, IOPacket_get_size(&pkt));

    const uint8_t pkt_size = IOPacket_get_size(&pkt);
    size_t ret = write_wait((uint8_t *)&pkt, pkt_size);

    if (ret != pkt_size) {
        debug("write failed2 %u %u %u %u\n", pkt_size, page, offset, ret);
        intermcu.protocol_fail_count++;
        return false;
    }

    // wait for the expected number of reply bytes or timeout
    if (!wait_timeout(4, 10)) {
        debug("no reply for %u/%u/%u\n", page, offset, count);
        intermcu.protocol_fail_count++;
        return false;
    }

    uint8_t *b = (uint8_t *)&pkt;
    uint8_t n = intermcu.rx_buff_length;
    for (uint8_t i=0; i<n; i++) {
        if (i < sizeof(pkt)) {
            rt_device_read(intermcu.puart, 0, &b[i], 1);
        }
    }

    if (pkt.code != CODE_SUCCESS) {
        debug("bad code %02x write %u/%u/%u %02x/%02x n=%u\n",
              pkt.code, page, offset, count,
              pkt.page, pkt.offset, n);
        intermcu.protocol_fail_count++;
        return false;
    }
    uint8_t got_crc = pkt.crc;
    pkt.crc = 0;
    uint8_t expected_crc = math_crc_crc8((const uint8_t *)&pkt, IOPacket_get_size(&pkt));
    if (got_crc != expected_crc) {
        debug("bad crc %02x should be %02x\n", got_crc, expected_crc);
        intermcu.protocol_fail_count++;
        return false;
    }
    if (intermcu.protocol_fail_count > IOMCU_MAX_REPEATED_FAILURES) {
        handle_repeated_failures();
    }
    intermcu.total_errors += intermcu.protocol_fail_count;
    intermcu.protocol_fail_count = 0;
    intermcu.protocol_count++;
    return true;
}

// write a single register
static inline bool write_register(uint8_t page, uint8_t offset, uint16_t v)
{
    return write_registers(page, offset, 1, &v);
}

/*
  discard any pending input
 */
static inline void discard_input(void)
{
    static uint8_t rec_buff[IOMCU_SERIAL_RX_BUFSZ];
    rt_device_read(intermcu.puart, 0, rec_buff, IOMCU_SERIAL_RX_BUFSZ);
    intermcu.rx_buff_length = 0;
}

/*
  write a packet, retrying as needed
 */
static rt_size_t write_wait(const uint8_t *pkt, uint8_t len)
{
    rt_size_t ret;

    ret = rt_device_write(intermcu.puart, 0, pkt, len);

    // 等待串口发送完成
    if (intermcu.tx_sem != NULL) {
        rt_sem_take(intermcu.tx_sem, RT_WAITING_FOREVER);
    }

    return ret;
}

/**
  * @brief       UART发送完成回调,DMA发送完成释放信号量
  * @param[in]   dev  
  * @param[in]   buffer  
  * @param[out]  
  * @retval      
  * @note        
  */
static rt_err_t uart_tx_done(rt_device_t dev, void* buffer)
{
    rt_err_t res = RT_EEMPTY;

    if (intermcu.tx_sem != NULL) {
        res = rt_sem_release(intermcu.tx_sem);
    }

    return res;
}

/**
  * @brief       UART接收回调函数
  * @param[in]   dev  
  * @param[in]   size  
  * @param[out]  
  * @retval      
  * @note        
  */
static rt_err_t uart_rx_indicate(rt_device_t dev, rt_size_t size)
{
    if (intermcu.puart == dev) {
        intermcu.rx_buff_length = size;
    }

    return RT_EOK;
}

/*
  wait for at least n bytes of incoming data, with timeout in
  milliseconds. Return true if n bytes are available, false if
  timeout
 */
static bool wait_timeout(uint16_t n, uint32_t timeout_ms)
{
    uint32_t t0 = time_millis();
    while (intermcu.rx_buff_length < n) {
        uint32_t now = time_millis();
        if (now - t0 >= timeout_ms) {
            break;
        }
        rt_thread_mdelay(timeout_ms - (now - t0));
    }

    return intermcu.rx_buff_length >= n;
}
/*
  we have had a series of repeated protocol failures to the
  IOMCU. This may indicate that the IOMCU has been reset (possibly due
  to a watchdog).
 */
static void handle_repeated_failures(void)
{
    if (intermcu.protocol_count < 100) {
        // we're just starting up, ignore initial failures caused by
        // initial sync with IOMCU
        return;
    }

    //INTERNAL_ERROR(AP_InternalError::error_t::iomcu_fail);
}

/*
  handle event failure
 */
static void event_failed(uint32_t event_mask)
{
    // wait 1ms then retry
    rt_thread_mdelay(1);
    rt_event_send(&intermcu.io_event, EVENT_MASK(event_mask));
}

// modify a single register
static bool modify_register(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits)
{
    uint16_t v = 0;
    if (!read_registers(page, offset, 1, &v)) {
        return false;
    }
    uint16_t v2 = (v & ~clearbits) | setbits;
    if (v2 == v) {
        return true;
    }
    return write_registers(page, offset, 1, &v2);
}

/*
  read RC input
 */
static void read_rc_input()
{
    uint16_t *r = (uint16_t *)&intermcu.rc_input;
    if (!read_registers(PAGE_RAW_RCIN, 0, sizeof(intermcu.rc_input)/2, r)) {
        return;
    }

    // GP TODO: rc().ignore_rc_failsafe()
    if (intermcu.rc_input.flags_failsafe) {
        intermcu.rc_input.flags_failsafe = false;
    }
    if (intermcu.rc_input.flags_rc_ok && !intermcu.rc_input.flags_failsafe) {
        intermcu.rc_last_input_ms = time_millis();
    }
}

/*
  read status registers
 */
static void read_status()
{
    uint16_t *r = (uint16_t *)&intermcu.reg_status;
    if (!read_registers(PAGE_STATUS, 0, sizeof(intermcu.reg_status)/2, r)) {
        intermcu.read_status_errors++;
        return;
    }
    if (intermcu.read_status_ok == 0) {
        // reset error count on first good read
        intermcu.read_status_errors = 0;
    }
    intermcu.read_status_ok++;

    check_iomcu_reset();

    if (intermcu.reg_status.flag_safety_off == 0) {
        // if the IOMCU is indicating that safety is on, then force a
        // re-check of the safety options. This copes with a IOMCU reset
        intermcu.last_safety_options = 0xFFFF;

        uint16_t options = brd_get_safety_button_options();
        if (intermcu.safety_forced_off && (options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON) == 0) {
            // the safety has been forced off, and the user has asked
            // that the button can never be used, so there should be
            // no way for the safety to be on except a IOMCU
            // reboot. Force safety off again
            iomcu_force_safety_off();
        }
    }

    uint32_t now = time_millis();

    if (now - intermcu.last_log_ms >= 1000U) {
        intermcu.last_log_ms = now;
#if 0
        if (AP_Logger::get_singleton()) {
// @LoggerMessage: IOMC
// @Description: IOMCU diagnostic information
// @Field: TimeUS: Time since system startup
// @Field: Mem: Free memory
// @Field: TS: IOMCU uptime
// @Field: NPkt: Number of packets received by IOMCU
// @Field: Nerr: Protocol failures on MCU side
// @Field: Nerr2: Reported number of failures on IOMCU side
// @Field: NDel: Number of delayed packets received by MCU
            AP::logger().WriteStreaming("IOMC", "TimeUS,Mem,TS,NPkt,Nerr,Nerr2,NDel", "QHIIIII",
                               AP_HAL::micros64(),
                               reg_status.freemem,
                               reg_status.timestamp_ms,
                               reg_status.total_pkts,
                               total_errors,
                               reg_status.num_errors,
                               num_delayed);
        }
#endif
#if IOMCU_DEBUG_ENABLE
        static uint32_t last_io_print;
        if (now - last_io_print >= 5000) {
            last_io_print = now;
            debug("t=%u num=%u mem=%u terr=%u nerr=%u crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%u\n",
                  now,
                  intermcu.reg_status.total_pkts,
                  intermcu.reg_status.freemem,
                  intermcu.total_errors,
                  intermcu.reg_status.num_errors,
                  intermcu.reg_status.err_crc,
                  intermcu.reg_status.err_bad_opcode,
                  intermcu.reg_status.err_read,
                  intermcu.reg_status.err_write,
                  intermcu.reg_status.err_uart,
                  intermcu.num_delayed);
        }
#endif // IOMCU_DEBUG_ENABLE
    }
}

/*
  check for IOMCU reset (possibly due to a watchdog).
 */
static void check_iomcu_reset(void)
{
    if (intermcu.last_iocmu_timestamp_ms == 0) {
        // initialisation
        intermcu.last_iocmu_timestamp_ms = intermcu.reg_status.timestamp_ms;
        console_printf("IOMCU startup\n");
        return;
    }
    uint32_t dt_ms = intermcu.reg_status.timestamp_ms - intermcu.last_iocmu_timestamp_ms;
    uint32_t ts1 = intermcu.last_iocmu_timestamp_ms;
    // when we are in an expected delay allow for a larger time
    // delta. This copes with flash erase, such as bootloader update
    //const uint32_t max_delay = hal.scheduler->in_expected_delay()?5000:500;
    const uint32_t max_delay = 1000;
    intermcu.last_iocmu_timestamp_ms = intermcu.reg_status.timestamp_ms;

    if (dt_ms < max_delay) {
        // all OK
        intermcu.last_safety_off = intermcu.reg_status.flag_safety_off;
        return;
    }
    intermcu.detected_io_reset = true;
    //INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset);
    console_printf("IOMCU reset t=%u %u %u dt=%u\n",
                        (unsigned)time_millis(), (unsigned)(ts1), (unsigned)(intermcu.reg_status.timestamp_ms), (unsigned)(dt_ms));

    if (intermcu.last_safety_off && !intermcu.reg_status.flag_safety_off && brd_get_soft_armed()) {
        uint16_t options = brd_get_safety_button_options();
        if (intermcu.safety_forced_off || (options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) == 0) {
            // IOMCU has reset while armed with safety off - force it off
            // again so we can keep flying
            iomcu_force_safety_off();
        }
    }
    intermcu.last_safety_off = intermcu.reg_status.flag_safety_off;

    // we need to ensure the mixer data and the rates are sent over to
    // the IOMCU
    if (intermcu.mixing.enabled) {
        trigger_event(IOEVENT_MIXING);
    }
    trigger_event(IOEVENT_SET_RATES);
    trigger_event(IOEVENT_SET_DEFAULT_RATE);
    if (intermcu.rate.oneshot_enabled) {
        trigger_event(IOEVENT_SET_ONESHOT_ON);
    }
    if (intermcu.rate.brushed_enabled) {
        trigger_event(IOEVENT_SET_BRUSHED_ON);
    }
    if (intermcu.rate.sbus_rate_hz) {
        trigger_event(IOEVENT_ENABLE_SBUS);
    }
    if (intermcu.pwm_out.safety_mask) {
        trigger_event(IOEVENT_SET_SAFETY_MASK);
    }
    intermcu.last_rc_protocols = 0;
}

/*
  read servo output values
 */
static void read_servo()
{
    if (intermcu.pwm_out.num_channels > 0) {
        read_registers(PAGE_SERVOS, 0, intermcu.pwm_out.num_channels, intermcu.pwm_in.pwm);
    }
}

// handling of BRD_SAFETYOPTION parameter
static void update_safety_options(void)
{
    uint16_t desired_options = 0;
    uint16_t options = brd_get_safety_button_options();
    if (!(options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
        desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_OFF;
    }
    if (!(options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
        desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_ON;
    }
    if (!(options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && brd_get_soft_armed()) {
        desired_options |= (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
    }
    if (intermcu.last_safety_options != desired_options) {
        uint16_t mask = (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
        uint32_t bits_to_set = desired_options & mask;
        uint32_t bits_to_clear = (~desired_options) & mask;
        if (modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, bits_to_clear, bits_to_set)) {
            intermcu.last_safety_options = desired_options;
        }
    }
}

// update enabled RC protocols mask
static void send_rc_protocols()
{
    // GP TODO:
    // const uint32_t v = rc().enabled_protocols();
    const uint32_t v = 1;
    if (intermcu.last_rc_protocols == v) {
        return;
    }
    if (write_registers(PAGE_SETUP, PAGE_REG_SETUP_RC_PROTOCOLS, 2, (uint16_t *)&v)) {
        intermcu.last_rc_protocols = v;
    }
}

/*------------------------------------test------------------------------------*/

#endif

